#include "tos_k.h"
#include "pico/multicore.h"
#include "hardware/sync.h"

uint32_t spinlock_itrstate[2];

__PORT__ uint32_t port_cpu_get_index(void)
{
    return get_core_num();
}

__PORT__ k_err_t port_smp_try_push_msg(uint8_t idx, uint32_t data)
{
    (void)idx; // We only have 2 cores so idx will not be used
    if(!multicore_fifo_wready())
        return K_ERR_SMP_PFIFO_FULL;
    multicore_fifo_push_blocking_inline(data);
    return K_ERR_NONE;
}

__PORT__ k_err_t port_smp_try_pop_msg(uint8_t idx, uint32_t* data)
{
    (void)idx; // We only have 2 cores so idx will not be used
    if(!multicore_fifo_rvalid())
        return K_ERR_SMP_PFIFO_EMPTY;
    *data = multicore_fifo_pop_blocking();
    return K_ERR_NONE;
}

__PORT__ k_err_t port_smp_global_lock(uint8_t idx)
{
    spinlock_itrstate[idx - PICO_SPINLOCK_ID_OS1] = spin_lock_blocking(spin_lock_instance(idx));
    return K_ERR_NONE;
}

__PORT__ k_err_t port_smp_global_unlock(uint8_t idx)
{
    spin_unlock(spin_lock_instance(idx), spinlock_itrstate[idx - PICO_SPINLOCK_ID_OS1]);
    return K_ERR_NONE;
}

__PORT__ uint8_t port_smp_global_is_locked(uint8_t idx)
{
    return is_spin_locked(spin_lock_instance(idx));
    return 0;
}
